/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief Dummy implementation for testing the RangingServer interface.
 * \author Antti Maula <antti.maula@tkk.fi>
 */
#include <stdio.h>
#include <stdlib.h>
#include "owndebug.h"
#include "RangingServer.hpp"
#include "binbag.h"
#include <set>

//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
static int verbose = ODINFO;
static volatile bool run = true;
static ownThreadHandle signalHandlerThread;
//*****************************************************************************
enum EDummyMode { KModeIncrement = 1, 
                  KModeZero = 2, 
                  KModeMin = 3, 
                  KModeMax = 4,
                  KModeCloseOpen = 5 };
//*****************************************************************************
using namespace MaCI;
//*****************************************************************************
static std::string macigroup = "Dummy";
static std::string groupinstance = "Dummy";
static ownMutexHandle activeInterfacesMutex = ownMutex_Init();
static std::set<CMaCI *> activeInterfaces;
//*****************************************************************************

static int SignalHandlerThread(void *)
{
  dPrint(8, "Signal handler thread (%ld) starting...", pthread_self());

  while (run) {
    // Now, wait for requested signals to arrive
    int sig;
    sigset_t mask;
    sigemptyset(&mask);
    sigaddset(&mask, SIGINT);
    sigaddset(&mask, SIGTERM);
    sigaddset(&mask, SIGPIPE);
    sigaddset(&mask, SIGHUP);
    sigwait(&mask, &sig);

    // Got signal, sigwait returned!
    dPrint(1,"Got Signal '%d' (%s)", sig, strsignal(sig));

    // What to do?
    switch(sig) {
    case SIGINT:
    case SIGTERM:
    case SIGPIPE:
    case SIGHUP:
      dPrint(1,"Terminating Module...");
      run = false;
      ownMutex_Lock(activeInterfacesMutex);
      for(EACH_IN_i(activeInterfaces)) {
        (*i)->Close();
      }
      activeInterfaces.clear();
      ownMutex_Unlock(activeInterfacesMutex);

      break;
      
    default:
      dPrint(1,"Unhandled signal! Ignore!");
      break;
    }
  }
  return 0;
}
//*****************************************************************************

static void InitSignalHandler(void)
{
  // Now; Set to BLOCK ALL SIGNALS
  sigset_t mask;
  sigemptyset(&mask);
  sigaddset(&mask, SIGINT);
  sigaddset(&mask, SIGTERM);
  sigaddset(&mask, SIGPIPE);
  sigaddset(&mask, SIGHUP);
  pthread_sigmask(SIG_BLOCK, &mask, NULL);
  
  // Start signal handler thread
  signalHandlerThread = ownThread_Create((void*)SignalHandlerThread, NULL);
}
//*****************************************************************************
//*****************************************************************************
void showUsage()
{
  fprintf(stderr,"\n");
  fprintf(stderr,"Usage:\tRanging_Dummy [OPTIONS]\n");
  fprintf(stderr,"  [-u addr]\t\t\tUse 'addr' as GIMnet AccessPoints address\n");
  fprintf(stderr,"  [-p port]\t\t\tUse 'port' as GIMnet AccessPoints port\n");
  fprintf(stderr,"  [-n name]\t\t\tUse 'name' as this nodes GIMnet name\n");
  fprintf(stderr,"  [-G name]\t\t\tSpecify the MaCIGroup for this module\n");
  fprintf(stderr,"  [-I name]\t\t\tSpecify the MaCIGroup Interface Instance\n");
  
  fprintf(stderr,"  [-v]\t\t\t\tIncrease verbosity by 1\n");
  fprintf(stderr,"  [-q]\t\t\t\tQuiet mode\n");
  fprintf(stderr, "\n\n");
}
//*****************************************************************************

int main(int argc, char *argv[]) 
{
  // GIMnet parameters
  std::string gimnetAP = "asrobo.hut.fi";
  int gimnetAPPort = 50002;
  std::string gimnetName = "";
  
  // Dummy parameter
  float beam_width = 0.1;
  float min = 0.1, max = 81.92;
  int beam_count = 180; //1000;
    EDummyMode mode = KModeIncrement;
  //  EDummyMode mode = KModeCloseOpen;
  int interval = 200;
  
  // Objects
  gimi::GIMI g;
  MaCICtrl::CMaCICtrlServer mcs(&g);
  MaCI::Ranging::CRangingServer rs(&mcs, 0); 

  // Init Debug lib
  debugInit();
  debugSetGlobalDebugLvl(verbose);
  const std::string logfile = std::string(argv[0]) + ".log";
  debugSetLogFilename(logfile.c_str());

  InitSignalHandler();

  // Greet
  fprintf(stderr,
          "\n%s - Hello\n", argv[0]);

  // Parse parameters
  bool parse = true;
  while(parse) {
    int c;
    c = getopt(argc, argv, "G:I:b:d:B:n:u:p:hqv");
    switch(c) {

      /////////////////////////////////
      ////// Application specific /////
      /////////////////////////////////
    case 'b':
      beam_count = atoi(optarg);
      dPrint(3,"Using Beam_count = %d", beam_count);
      break;
      

      /////////////////////////////////
      ///// GIMnet parameters /////////
      /////////////////////////////////
    case 'n':
      gimnetName = optarg;
      dPrint(3,"Using GIMnet Name '%s'", optarg);
      break;

    case 'u':
      gimnetAP = optarg;
      dPrint(3,"Using GIMnetAP address '%s'", optarg);
      break;

    case 'p':
      gimnetAPPort = atoi(optarg);
      dPrint(3,"Using GIMnetAP port '%d'", gimnetAPPort);
      break;

    case 'G':
      macigroup = optarg;
      dPrint(3,"Using MaCI-Group '%s'", optarg);
      break;

    case 'I':
      groupinstance = optarg;
      dPrint(3,"Using MaCI-Group Interface Instance '%s'", optarg);
      break;

      //////////////////////////////////
      //// GENERIC:                 ////
      //////////////////////////////////
    case 'v':
      // Increase only if not previously set to 0 (by 'quiet')
      if (verbose > 0)
        debugSetGlobalDebugLvl(++verbose);
      break;
      
    case 'q':
      verbose = 0;
      debugSetGlobalDebugLvl(0);
      break;
      
    case 'h':
    case '?':
      showUsage();
      exit(1);
      break;
      
    case -1:
      parse = false;
      break;
    }
  }
  
  /////////////////////////////////////////////////////////////////////////////
  // Modify gimetName if unset (length == zero)
  /////////////////////////////////////////////////////////////////////////////
  if (gimnetName.empty()) {
    gimnetName = MaCI::GetStaticClientName(argv[0]+macigroup+groupinstance);
  }
   
  int r;
  if ( (r = g.connectToHubEx(gimnetAP, gimnetAPPort, gimnetName) ) != GIMI_OK) {
    dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
           gimnetAP.c_str(), 
           gimnetAPPort, 
           gimnetName.size()?gimnetName.c_str():"<anonymous>",
           gimi::getGimiErrorString(r).c_str());
    
  } else {
    dPrint(2,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
           gimnetAP.c_str(), 
           gimnetAPPort, 
           gimnetName.size()?gimnetName.c_str():"<anonymous>",
           gimi::getGimiErrorString(r).c_str());

    // Set Group & Instance.
    mcs.SetGroupName(macigroup);
    rs.SetInterfaceInstanceName(groupinstance);

    // Set limits & position.
    rs.SetDeviceLimits(min, max);
    rs.SetDevicePosition(1.0, 2.0, 3.0, 4.0); // App specific. Not set.
    MaCI::EMaCIError e;
    if ( (e = rs.Open()) != MaCI::KMaCIOK) {
      dPrint(1,"Failed to Open RangingServer instance! (%s)",
             GetErrorStr(e).c_str());
      
    } else {
      activeInterfaces.insert(&rs);

      int count = 1;
      while (run) {
        ownMutex_Lock(activeInterfacesMutex);
        // Create RangingData container
        MaCI::Ranging::CRangingData rdata;
        gim::binbag::CBinBag bag;
        rdata.EncodeTo(&bag);
        float distance = 0.00;
        float angle = 0.00;
	count++;
	count = count%100;

        // Set Timestamp and DistanceHeader
        //rdata.SetTimestamp(MaCI::Common::TTimestamp(0, 0));
	MaCI::Common::TTimestamp stamp;
	stamp.SetToCurrentTime();
	rdata.SetTimestamp(stamp);
        rdata.SetDistanceHeader(MaCI::Ranging::TDistanceHeader(beam_width, beam_count));
	
      
        // Add distances
	distance = 1;
        for (int i=0; i < beam_count; ++i) {
          switch(mode) {
          case KModeCloseOpen: // No specific needs for this.
          case KModeIncrement:
            distance += 0.1; //count + (i%10)/10;
            angle += 0.1;
            break;
          case KModeZero:
            distance += 1.0;
            angle += 1.0;
            break;
          case KModeMin:
            distance = min;
            angle = 0.0;
            break;
          case KModeMax:
            distance = max;
            angle = 0.0;
            break;
          }

          rdata.AddDistance(MaCI::Ranging::TDistance(angle, distance));
        }
      
        // Send the newly constructed DistanceArray to subscribers
        rs.SendDistanceArrayEvent(rdata);
      
        // print
        dPrint(4,"DistanceArrayEvent sent. (%u Distance-elements)", rdata.GetDistanceElementCount());

        // Close!
        if (mode == KModeCloseOpen) {
          dPrint(ODTEST,"Interface closing...");
          rs.Close();
          dPrint(ODTEST,"Waiting 10 seconds...");
          ownSleep_ms(1000 * 10);
    
          dPrint(ODTEST,"Interface ReOpening");
          rs.Open();

          dPrint(ODTEST,"Waiting 10 seconds...");
          ownSleep_ms(1000 * 10);
        }
      
        ownMutex_Unlock(activeInterfacesMutex);
        // Sleep
        ownSleep_ms(interval);
      }

      ownMutex_Lock(activeInterfacesMutex);
      for(EACH_IN_i(activeInterfaces)) {
        (*i)->Close();
      }
      activeInterfaces.clear();
      ownMutex_Unlock(activeInterfacesMutex);
    }
  }

  return 0;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************


